#include "csgcontroller.h"

CSGController::CSGController()
{
    m_poly = Poly();
}

void CSGController::readCSGFile(QString fileName){
    //Read the CSG file and creates the CSGTree.

    m_csgTree = CSGTree();

    int iNprimitives;
    vector<Tree::iterator> primitives;
    CSGNode *pNode;
    QString primitiveName;
    Tree *tempTree;

    QFile csgFile(fileName);
    csgFile.open(QIODevice::ReadOnly | QIODevice::Text);
    QTextStream in(&csgFile);
    in.setRealNumberNotation(QTextStream::FixedNotation);

    in >> iNprimitives;
    for(int i = 0 ; i < iNprimitives ; i++){
        in >> primitiveName;
        if(primitiveName == "box"){
            double fXMin , fXMax , fYMin , fYMax , fZMin , fZMax;
            in >> fXMin >> fXMax >> fYMin >> fYMax >> fZMin >> fZMax;
            pNode = new CSGBox(fXMin , fXMax , fYMin , fYMax , fZMin , fZMax);
        }else if(primitiveName == "cylinder"){
            double fRadius , fHeight;
            in >> fRadius >> fHeight;
            pNode = new CSGCylinder(fRadius , fHeight);
        }else if(primitiveName == "sphere"){
            double dRadius;
            in >> dRadius;
            pNode = new CSGSphere(dRadius);
        }
        else if(primitiveName == "translation"){
            double dX,dY,dZ;
            in >> dX >> dY >> dZ;
            Vector translationV = Vector(dX, dY , dZ);
            pNode = new CSGTransformation(translationV);
        }
        else if(primitiveName == "scaling"){
            double dFactor;
            in >> dFactor;
            pNode = new CSGTransformation(dFactor);
        }
        else if(primitiveName == "rotation"){
            int iAxis;
            double dAngle;
            in >> iAxis >> dAngle;
            pNode = new CSGTransformation((Axis)iAxis , dAngle);
        }
        else{
            cerr << "Error in the input CSGFile.";
            exit(-1);
        }

        tempTree = new Tree(pNode);
        primitives.push_back(tempTree->begin());
    }

    //Node that signs for the "(" character.
    CSGNode *invalidatedNode = new CSGNode();
    invalidatedNode->m_bInvalidateNode = true;
    tempTree = new Tree(invalidatedNode);
    int iIndexInvalidatedNode = primitives.size();
    primitives.push_back(tempTree->begin());

    QString opMember;
    bool bConversionOk = false;
    int iIndex;
    stack<Tree::iterator> treeStack , childrenStack;
    Tree::iterator it;

    while(!in.atEnd()){
        in >> opMember;
        //cout << "operand = " << opMember.toStdString() << endl;
        iIndex = opMember.toInt(&bConversionOk , 10);
        if(opMember != ")"){
            //cout << "not ) case" << endl;
            if(opMember == "("){
                iIndex = iIndexInvalidatedNode;
            }
            else if(opMember == "\\"){
                iIndex = primitives.size();
                tempTree = new Tree(new CSGDiff());
                primitives.push_back(tempTree->begin());
            }
            else if(opMember == "-"){
                iIndex = primitives.size();
                tempTree = new Tree(new CSGIntersection());
                primitives.push_back(tempTree->begin());
            }
            else if(opMember == "+"){
                iIndex = primitives.size();
                tempTree = new Tree(new CSGUnion());
                primitives.push_back(tempTree->begin());
            }
            else if(!bConversionOk){
                cout << "Error processing the CSG file." << endl;
                exit(-1);
            }
            treeStack.push(primitives.at(iIndex));

            /*
            //only debug...
            cout << "stack: " << endl;
            stack<Tree::iterator> tempTreeStack(treeStack);
            while(!treeStack.empty()){
                Tree::iterator iter = treeStack.top();
                treeStack.pop();
                pNode = (*iter);
                string sType = string(typeid(*pNode).name());

                cout << sType << endl;
            }
            treeStack = tempTreeStack;
            cout << endl;
            //end debug*/
        }
        else{
            //cout << ") case" << endl;
            it = treeStack.top();
            treeStack.pop();

            while(!(*it)->m_bInvalidateNode){
                childrenStack.push(it);
                it = treeStack.top();
                treeStack.pop();
            }
            it = treeStack.top();

            while(!childrenStack.empty()){
                m_csgTree.m_csgTree.append_child(it , childrenStack.top());
                childrenStack.pop();    
            }
            treeStack.pop();
            treeStack.push(it);

            /*//only debug...
            cout << "stack: " << endl;
            stack<Tree::iterator> tempTreeStack(treeStack);
            while(!treeStack.empty()){
                Tree::iterator iter = treeStack.top();
                treeStack.pop();
                pNode = (*iter);
                string sType = string(typeid(*pNode).name());

                cout << sType << endl;
            }
            treeStack = tempTreeStack;
            cout << endl;
            //end debug*/
        }
        in.skipWhiteSpace();
    }
    if(treeStack.size() == 1){
        m_csgTree.m_csgTree = treeStack.top();
        treeStack.pop();
    }
    else{
        cout << "Error processing the CSG file." << endl;
        exit(-1);
    }
    csgFile.close();
}

long int CSGController::calcIsoSurface(XYZ origin , int aiDims[3] , double adSpacing[3]) //scalar field dimensions
{
    /*Creates the volumetric data and applies the marching tetrahedrons. Return the number of triangles*/
    m_vTriangles.clear();

    //Creating the grids and splitting them into tetrahedrons wich the marching tetrahedrons algorithm is appied.
    double dIso = 0.0;
    int iNumTri;
    GRIDCELL grid;
    TRIANGLE aTriangles[2];
    for (int iZ = 0; iZ < aiDims[2] - 1; iZ++){
        for (int iY = 0; iY < aiDims[1] - 1; iY++){
            for (int iX = 0; iX < aiDims[0] - 1; iX++){

                XYZ p0(origin.x + iX * adSpacing[0] , origin.y + iY * adSpacing[1] , origin.z + iZ * adSpacing[2]);
                grid.p[0] = p0;
                grid.val[0] = m_csgTree.calcScalarFieldValue(p0);

                XYZ p1(origin.x + (iX + 1) * adSpacing[0] , origin.y + iY * adSpacing[1] , origin.z + iZ * adSpacing[2]);
                grid.p[1] = p1;
                grid.val[1] = m_csgTree.calcScalarFieldValue(p1);

                XYZ p2(origin.x + (iX + 1) * adSpacing[0] , origin.y + iY * adSpacing[1] , origin.z + (iZ + 1) * adSpacing[2]);
                grid.p[2] = p2;
                grid.val[2] = m_csgTree.calcScalarFieldValue(p2);

                XYZ p3(origin.x + iX * adSpacing[0] , origin.y + iY * adSpacing[1] , origin.z + (iZ + 1) * adSpacing[2]);
                grid.p[3] = p3;
                grid.val[3] = m_csgTree.calcScalarFieldValue(p3);

                XYZ p4(origin.x + iX * adSpacing[0] , origin.y + (iY + 1) * adSpacing[1] , origin.z + iZ * adSpacing[2]);
                grid.p[4] = p4;
                grid.val[4] = m_csgTree.calcScalarFieldValue(p4);

                XYZ p5(origin.x + (iX + 1) * adSpacing[0] , origin.y + (iY + 1) * adSpacing[1] , origin.z + iZ * adSpacing[2]);
                grid.p[5] = p5;
                grid.val[5] = m_csgTree.calcScalarFieldValue(p5);

                XYZ p6(origin.x + (iX + 1) * adSpacing[0] , origin.y + (iY + 1) * adSpacing[1] , origin.z + (iZ + 1) * adSpacing[2]);
                grid.p[6] = p6;
                grid.val[6] = m_csgTree.calcScalarFieldValue(p6);

                XYZ p7(origin.x + iX * adSpacing[0] , origin.y + (iY + 1) * adSpacing[1] , origin.z + (iZ + 1) * adSpacing[2]);
                grid.p[7] = p7;
                grid.val[7] = m_csgTree.calcScalarFieldValue(p7);

                iNumTri = MarchingTetrahedrons::PolygoniseTri(grid,dIso,aTriangles,0,2,3,7); addTriangles(aTriangles , iNumTri ,  m_vTriangles);
                iNumTri = MarchingTetrahedrons::PolygoniseTri(grid,dIso,aTriangles,0,2,7,6); addTriangles(aTriangles , iNumTri ,  m_vTriangles);
                iNumTri = MarchingTetrahedrons::PolygoniseTri(grid,dIso,aTriangles,0,4,6,7); addTriangles(aTriangles , iNumTri ,  m_vTriangles);
                iNumTri = MarchingTetrahedrons::PolygoniseTri(grid,dIso,aTriangles,0,6,1,2); addTriangles(aTriangles , iNumTri ,  m_vTriangles);
                iNumTri = MarchingTetrahedrons::PolygoniseTri(grid,dIso,aTriangles,0,6,4,1); addTriangles(aTriangles , iNumTri ,  m_vTriangles);
                iNumTri = MarchingTetrahedrons::PolygoniseTri(grid,dIso,aTriangles,5,6,1,4); addTriangles(aTriangles , iNumTri ,  m_vTriangles);
            }
        }
    }

    constructPoly();
    return m_poly.size_of_facets();// m_vTriangles.size(); polygon soup
}

void CSGController::addTriangles(const TRIANGLE *aTriangles , int iNumTri ,  vector<TRIANGLE> &m_vTriangles){
    //Add triangles for the polygon soup of triangles, i.e., triangles without connectivity.
    for(int iI = 0 ; iI < iNumTri ; iI++){
        m_vTriangles.push_back(aTriangles[iI]);
    }
}

void CSGController::constructPoly(){
    //Constructs the halfedge of the polyhedron.
    m_poly.clear();
    HeBuilder<HalfedgeDS> heBuilder(&m_vTriangles , m_vTriangles.size());
    m_poly.delegate(heBuilder);
    //Build plane equations for each facet.
    std::transform( m_poly.facets_begin(), m_poly.facets_end(), m_poly.planes_begin(), Plane_equation());
    calcPolyNormals();
    cout << "facets = " << m_poly.size_of_facets() << " vertices = " << m_poly.size_of_vertices() << " halfedges = " << m_poly.size_of_halfedges() << endl;
}

void CSGController::calcPolyNormals(){
    //Calculates the normals for each vertex in the mesh. It is a area weighted average of the normals of all the incident faces.

    //Calculating the area weights...
    Facet_iterator fIt;
    Triangle tri;
    Halfedge_handle heIt;
    for(fIt = m_poly.facets_begin() ; fIt != m_poly.facets_end() ; fIt++){
        heIt = fIt->halfedge();
        tri = Triangle(heIt->vertex()->point() , heIt->next()->vertex()->point() , heIt->next()->next()->vertex()->point());
        fIt->dArea = sqrt(tri.squared_area());
    }

    //Calculating the normals...
    Vertex_iterator vIt;
    Halfedge_vertex_circulator heCirc;
    Vector normal;
    Facet_handle hFacet;
    double dWeight = 0.0 , dWeightSum = 0.0;
    double dX = 0.0 , dY = 0.0 , dZ = 0.0;

    for(vIt = m_poly.vertices_begin(); vIt != m_poly.vertices_end(); vIt++){
        heCirc = vIt->vertex_begin();
        normal = Vector(0.0 , 0.0 , 0.0);
        dWeightSum = 0.0;
        do{
            hFacet = heCirc->facet();
            if(hFacet != NULL){ //Sometimes CGAL returns a NULL reference to a facet. Probably a bug...
                dWeight = hFacet->dArea;
                dWeightSum += dWeight;
                normal = normal + dWeight * heCirc->facet()->plane().orthogonal_vector();
            }
        }
        while(++heCirc != vIt->vertex_begin());
        normal = normal / dWeightSum;//vIt->vertex_degree(); //averaging
        dX = normal.x(); dY = normal.y() ; dZ = normal.z();
        vIt->normal = normal / sqrt(dX * dX + dY * dY + dZ * dZ); //normalizing
    }
}

void CSGController::drawPoly(int iTriangleType , bool bShowAll){
    glColor3f(1.0f , 0.0f , 0.0f);
    glPolygonMode(GL_FRONT_AND_BACK , GL_FILL);

    //Half edge version
    Facet_iterator fIt;
    Halfedge_handle hHe;
    Point p[3];
    Vector normals[3];

    for(fIt = m_poly.facets_begin() ; fIt != m_poly.facets_end() ; fIt++){
        if(fIt->is_triangle()){
            hHe = fIt->halfedge();
            p[0] = hHe->vertex()->point();
            normals[0] = hHe->vertex()->normal;
            p[1] = hHe->next()->vertex()->point();
            normals[1] = hHe->next()->vertex()->normal;
            p[2] = hHe->prev()->vertex()->point();
            normals[2] = hHe->prev()->vertex()->normal;
            //cout << "(" << p[i].x() << "," << p[i].y() << "," << p[i].z() << ")";

            glBegin(GL_TRIANGLES);
                glNormal3d(normals[0].x() , normals[0].y() , normals[0].z());
                glVertex3d(p[0].x() , p[0].y() , p[0].z());

                glNormal3d(normals[1].x() , normals[1].y() , normals[1].z());
                glVertex3d(p[1].x() , p[1].y() , p[1].z());

                glNormal3d(normals[2].x() , normals[2].y() , normals[2].z());
                glVertex3d(p[2].x() , p[2].y() , p[2].z());
            glEnd();
        }
        else{
            cout << "Error... facet isn't a triangle!" << endl;
        }
    }
    /*
    //Polygon soup version
    vector<TRIANGLE>::iterator it;
    for(it = m_vTriangles.begin() ; it != m_vTriangles.end() ; it++){
        if(it->type == iTriangleType || bShowAll){
        glNormal3d(it->normal.x , it->normal.y , it->normal.z);
        glBegin(GL_TRIANGLES);
            glVertex3d(it->p[0].x , it->p[0].y , it->p[0].z);
            glVertex3d(it->p[1].x , it->p[1].y , it->p[1].z);
            glVertex3d(it->p[2].x , it->p[2].y , it->p[2].z);
        glEnd();
        }
    }*/
}
